// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MapCache.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Class that caches points sets obtained by ray tracing before the get converted to surface hdp.
    /// Such point sets can be reused by proximal particles in generating their own surface hdp without ray tracing again.
    /// </summary>
    /// <typeparam name="T">The type of objects to cache</typeparam>
    public class MapCache<T>
    {
        /// <summary>
        /// The default size of the cache
        /// </summary>
        private const int DefaultMaxSize = 1024;

        /// <summary>
        /// The estimated number of threads that will update the cache concurrently
        /// </summary>
        private const int ConcurrencyLevel = 16;

        /// <summary>
        /// The maximum size of the cache. Once this size is reached, the cache dumps old entries
        /// </summary>
        private int maxSize;

        /// <summary>
        /// Number of map cells per cache entry in each direction (e.g. 5 means cache buckets of 5x5 map cells).
        /// </summary>
        private double resolution;

        /// <summary>
        /// Orientation resolution of a cache entry (e.g. 2 degrees).
        /// </summary>
        private double angularResolution;

        /// <summary>
        /// The internal cache of surface points for each visited pose
        /// </summary>
        private ConcurrentDictionary<long, Tuple<Pose2D, T>> cache;

        /// <summary>
        /// The history of cache entries, used to trim the cache once maxSize is reached.
        /// </summary>
        private ConcurrentQueue<long> cacheKeys;

        /// <summary>
        /// Initializes a new instance of the MapCache class.
        /// </summary>
        /// <param name="resolution">Number of map cells per cache entry in each direction (e.g. 5 means cache buckets of 5x5 map cells).</param>
        /// <param name="angularResolution">Orientation resolution of a cache entry (e.g. 2 degrees).</param>
        /// <param name="maxSize">The maximum size of the cache. Once this size is reached, the cache dumps old entries</param>
        public MapCache(double resolution, double angularResolution, int maxSize = DefaultMaxSize)
        {
            this.maxSize = maxSize;
            this.resolution = resolution;
            this.angularResolution = angularResolution;
            this.cache = new ConcurrentDictionary<long, Tuple<Pose2D, T>>(ConcurrencyLevel, maxSize);
            this.cacheKeys = new ConcurrentQueue<long>();
        }

        /// <summary>
        /// Returns an entry from the cache if a matching one is present, or adds a new entry using the specified delegate
        /// </summary>
        /// <param name="pose">The pose to look up</param>
        /// <param name="valueFactory">The delegate that can create a new cache value if needed. Note: there is no guarantee that the value returned by the delegate is added to the cache.</param>
        /// <returns>The matching value from the cache</returns>
        public Tuple<Pose2D, T> GetOrAdd(Pose2D pose, Func<Pose2D, T> valueFactory)
        {
            // remove some cached entries if the cache is getting too big. Because of concurrency, we might remove more than one entry, which is ok.
            if (this.cacheKeys.Count >= this.maxSize)
            {
                long keyToRemove;
                if (this.cacheKeys.TryDequeue(out keyToRemove))
                {
                    Tuple<Pose2D, T> removed;
                    this.cache.TryRemove(keyToRemove, out removed);
                }
            }

            // if the entry is present, use it without taking a lock
            long key = this.KeyFromPose(pose);
            Tuple<Pose2D, T> generatedValue = null;
            Tuple<Pose2D, T> result;
            if (!this.cache.TryGetValue(key, out result))
            {
                // take a lock to prevent multiple threads from doing the same work
                lock (this.cache)
                {
                    // note that we could get here after some other thread has already added the value
                    result = this.cache.GetOrAdd(
                        key,
                        k =>
                        {
                            T value = valueFactory(pose);
                            generatedValue = Tuple.Create(pose, value);
                            return generatedValue;
                        });

                    if (generatedValue == result)
                    {
                        // the new value was added, make sure to track the key
                        this.cacheKeys.Enqueue(key);
                    }
                }
            }

            // if we did the work to generate a value for this pose, return that value instead of the cached one, 
            // since the generated value might not make it into the cache (because of concurrency) and thus there is no guarantee that result == generatedValue
            return (generatedValue != null) ? generatedValue : result;
        }

        /// <summary>
        /// Clears the cache. Cannot be called concurrently with GetOrAdd.
        /// </summary>
        public void Clear()
        {
            this.cacheKeys = new ConcurrentQueue<long>();
            this.cache.Clear();
        }

        /// <summary>
        /// Generates a key given a pose and knowing the resolution of the cache
        /// </summary>
        /// <param name="pose">The pose to generate a key for</param>
        /// <returns>The cache key corresponding to the pose</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private long KeyFromPose(Pose2D pose)
        {
            return (((long)(pose.Heading / this.angularResolution))  << 48) + ((long)(pose.X / this.resolution) << 24) + (long)(pose.Y / this.resolution);
        }
    }
}
